package at.fhooe.engine.pathfinding;

import java.awt.Point;
import java.awt.geom.Point2D;
import java.util.ArrayList;

import at.fhooe.engine.entity.Entity;
import at.fhooe.engine.entity.Updateable;
import at.fhooe.engine.pandora.Game;
import at.fhooe.engine.physics.CircleBody;
import at.fhooe.engine.physics.PhysicBody;
import at.fhooe.engine.physics.forces.DampingForce;
import at.fhooe.engine.physics.forces.DirectionAngularForce;
import at.fhooe.engine.physics.forces.Force;
import at.fhooe.engine.physics.forces.RoationForce;
import at.fhooe.enginge.pathfinding.help.SteeringObject;

public class Steering implements Updateable{

	private final double m_PRotFactor = 2.000;
	private final double m_IRotFactor = 0.0000;
	private final double m_DRotFactor = 100.0;
	private final double m_PMovFactor = 5.802;
	private final double m_IMovFactor = 0.10;
	private final double m_DMovFactor = 1000.0;
	
	private PhysicBody m_EntityPhysic;
	private ObstacleAvoidance m_Avoid;
	
	private Point m_Target;
	private Force lastRotForce;
	private Force lastMovForce;
	
	private double m_SumErrorM = 0.0;
	private double m_SumErrorR = 0.0;
	private double m_previousErrorM = 0.0;
	private double m_previousErrorR = 0.0;
	
	private double time = 2.0;
	
	private double m_Speed = 0.0;
	private double m_Angle = 0.0;
	
	
	public Steering(Entity object) {
		m_EntityPhysic = object.getPhysicBody();
		m_Avoid = new ObstacleAvoidance(this, object);
	}
	
	public void goToTarget(SteeringObject target) {
		
		m_Speed = target.speed;
		m_Angle = target.angle;
//		m_SumErrorM = 0.0;
//		m_SumErrorR = 0.0;
//		m_previousErrorM = 0.0;
//		m_previousErrorR = 0.0;
		//System.out.println("Neuer winkel "+m_Angle);
	}

	public void stop()
	{
		m_Speed = 0.0;
		m_Angle = 0.0;
		m_EntityPhysic.removeAllForces();
		m_EntityPhysic.setVelocity(0, 0, 0);
		m_EntityPhysic.setAngularVelocity(0);
		m_EntityPhysic.addForce(new DampingForce(3.0, 3.0, 0.0));
	}
	
	@Override
	public void update() {
		m_Avoid.update();
		if(m_Speed != 0.0 || m_Angle != 0.0) 
		{
			
			time -= Game.getInstance().getTimeSinceLastTick();
			if(time < 0.0)
			{
				m_SumErrorM = 0.0;
				m_SumErrorR = 0.0;
				m_previousErrorR = 0.0;
				time = 2.0;
			}
			
			
			//Movement
			//Integral
			m_SumErrorM += m_previousErrorM;
			double currentErrorM = m_Speed;
			//Differential
			double dEM = currentErrorM - m_previousErrorM;
			double forceM = m_PMovFactor * currentErrorM + m_IMovFactor * m_SumErrorM + m_DMovFactor * dEM;
			if(forceM < 0)
			{
				forceM = Math.max(forceM, -80);
			} else
			{
				forceM = Math.min(forceM, 80);
			}
			//Apply movement force to body
			m_EntityPhysic.removeForce(lastMovForce);
			lastMovForce = new DirectionAngularForce(forceM);
			m_EntityPhysic.addForce(lastMovForce);
			
			m_previousErrorM = currentErrorM;
			
			//Rotation
			m_SumErrorR += m_previousErrorR;
			double currentErrorR = m_Angle;
			if(currentErrorR < -Math.PI)
			{
//				currentErrorR += 2*Math.PI;
			}
			
			//Differential
			double dER = currentErrorR - m_previousErrorR;
			double forceR = m_PRotFactor * currentErrorR + m_IRotFactor * m_SumErrorR + m_DRotFactor * dER;
			
			if(forceR < 0)
			{
				forceR = Math.max(forceR, -3);
			} else
			{
				forceR = Math.min(forceR, 3);
			}
//			System.out.println(forceR);
//			System.out.println(m_Speed);
			//Apply rotation force to body
			m_EntityPhysic.removeForce(lastRotForce);
			lastRotForce = new RoationForce(-1, forceR);
			m_EntityPhysic.addForce(lastRotForce);
			
			m_previousErrorR = currentErrorR;
		}
	}

	public SteeringStrategy getStrategy() {
		return m_Avoid.getStrategy();
	}
	
	public ObstacleAvoidance getObstacleAvoidance(){
		return m_Avoid;
	}
}
